/////////////////////////////////////////////////////////////////////////////
//
// The Neurosciences Institute, 2011
//
// Project:
//      Conscious Artifact
//
// Author:
//       Rich Martin, et al
//
// Description:
//       world class encapsulates the world.
//       world::interact() allows you to ineract with the robot/environment.
//       Ideally, all communicating with the robot would be hidden inside this object
//       s.t. if a particular processor needs to get/set some value, it will be 
//       available from this object, even if from a romote host.
//
//       For now, we can have each node simulate an arm redundantly.
//
/////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <sstream>
#include <algorithm>
#include <vector>
#include <map>
#include <cmath>
#include <mpi.h>
#include "motormap.h"
#include "groups.h"
#include "main.h"

using namespace std;

#ifdef SIMULATED_ROBOT
#define APEX2
#else
#define APEX3
#endif // SIMULATED_ROBOT


#ifdef APEX1
#include "robot_real.h"
#endif

#ifdef APEX2
#include "robot_sim.h"
#endif

#ifdef APEX3
#include "apex_robot.h"
#endif

extern float x[][3];
							
extern float DA; //dopamine excitation
extern float DA_decay_rate;
extern float sd_decay_rate;
extern int APEX_REWARD;
bool reward_button_pressed = 0;

/****** RGM: Expt. time settings ******/	
int ms_per_sec = 1000;												

//Expt. time settings	
int start_no_reset_training_sec = 160;//20=>quick tests;//originally 80 for hierarchical model
int first_seq_stim_start_sec = 40;	
int testing_start_time_sec = 590;//270;//+320;
int training_end_time_sec = testing_start_time_sec-1;																
int M1_stim_end_time_sec = 590+320; //1000;//continue stimulation ad infinitum
int V1_stim_start_time_sec = 1000;//1000
int V1_stim_end_time_sec = 1000;//testing_start_time_sec;//32;
int training_sample_duration_ms = 1000;
int testing_sample_duration_ms = 1000;

#ifdef SIMULATED_ROBOT
bool kill_sim_at_specified_time = true;//turn to false after training is over
#else
bool kill_sim_at_specified_time = true;
#endif
int kill_sim_time_sec = 1225;//testing_start_time_sec + 90;//testing_start_time_sec; //32; //end of preliminary training

int reset_window_training = training_sample_duration_ms;
int reset_offset_training_A = 50; //training_sample_duration_ms - 1
int reset_offset_training_B = 100; //training_sample_duration_ms - 1; //50
int reset_offset_training_C = 0;// 100, 0
int reset_offset_training_D = 10;// 100, 0

int reset_window_testing = testing_sample_duration_ms;
int reset_offset_testing_A = 50; //testing_sample_duration_ms - 1; 
int reset_offset_testing_B = 100;//testing_sample_duration_ms - 1; //50
int reset_offset_testing_C = 0;//0
int reset_offset_testing_D = 10;//0

/* Rotation Station Definitions */
unsigned int rot_cntr = 0;
//float pose_matrix[] = { 0, 45, 90, 135 };//0, 90, 180, 270
float pose_matrix[] = { 0, 90, 180, 270 };
bool wheel = false; // false;

/* Robot Mental Rotation Parameters */
int pattern_A = 0;//global
int pattern_A_sec = 0;//
int pattern_B = 0;//global
int pattern_B_sec = 1500;
int rotate_head_sec_a = 1000;
int rotate_head_sec_b = 3000; 
int current_pattern = 0;
int trial_counter = -1;
int record_counter = 0;

// ********************************************************************* 
int start_time_ms = 170000;//testing_start_time_sec*ms_per_sec;
int pattern_trial_duration = 5000;//1000;//5000;
int lookat_A_ms = 0;
int lookaway_A_ms = 1000;//900;//1000;
int lookat_B_ms = 2000;
int lookaway_B_ms = 3000;
bool match_flag = false;
bool first_match = false;
// ********************************************************************* 
int min_pattern = 0;
int max_pattern = 3;
int action_pattern = 0;
bool motor_flag = false;
unsigned int head_pose = 0;
unsigned int head_pose_home = 443;
unsigned int head_pose_rotate = 275;//275;//right lower, left higher
unsigned int head_speed = 800;

//float max_degrees = 300.0;
//unsigned int resolution = 1023;   
//unsigned int pos_raw = (unsigned int)( (pos_degrees / max_degrees) * resolution);
//unsigned int rot_sta_poses[] = { 0, 306, 613, 920 };
unsigned int rot_sta_poses[] = { 0, 306, 613, 920, 0, 306, 613, 920 };
unsigned int blinder_up  = 360;
unsigned int blinder_down = 30;

unsigned int BLINDER_ID = 18;
unsigned int ROTATION_ID = 19;
unsigned int SPINNER_ID = 20;
unsigned int spinner_object1 = 20;
unsigned int spinner_object2 = 645;

ofstream DA_debugger;
ofstream button_timings;

//orignal home positions float home_pose_radians[] = { 0.851359, 1.08146, 0.32725 }; //shoulder, deltoid, elbow
float home_pose_radians[] = { 1.0, 1.2, 1.5 }; //shoulder, deltoid, elbow
float target_pose_radians[] = { 0.102142, 0.406207, 0.611036 }; //shoulder, deltoid, elbow
float start_pose = 0.0;
float current_pose = 0.0;
float target_threshold = 0.05; //radians
float movement_threshold = 0.25; //radians
float M1_stim_current = 0;
bool arm_moved_to_target = false;
bool arm_moved = false;
bool MTS_active = false;
bool coin_flip = false;
bool is_match_trial = false;
int conditioning_cnt = 0;

//#define start_dynamics
#define Save_Dynamics_Every_Second
/**************************************/

class electrode
{
 public:
  float position[3];
  int start_msec;
  int stop_msec;
  float dist;
  float stim_current;
};

//*******************************************
float dist(float x[3], float y[3], int n=3)
{
  float d = 0.0f;
	
  for(int i = 0; i < n; i++)
    {
      d+=(x[i]-y[i])*(x[i]-y[i]);
    }
	
  return sqrt(d);
}

//world clas wrapper for interact functions, etc.
class world 
{
 private:
  vector<float> mfr;
  vector<electrode> electrodes;
	
 public:
#ifdef APEX3
  typedef ::nsi::thinklink::scoped_ptr< ::nsi::thinklink::device > device;
#endif
	
  void init(group igroup_data[MAX_AREAS][MAX_TYPES]) 
  {
    sim_end_time_sec = 17000;
    
    #ifdef APEX3
    
    /* Rotation Training Settings
    trial_start_ms = start_time_ms;
    trial_duration = 1000;//pattern_trial_duration;
    lookat_A = 0;//lookat_A_ms;
    lookaway_A = 900;//lookaway_A_ms;
    lookat_B = 1000;//lookat_B_ms;
    lookaway_B = 1000;//lookaway_B_ms;
    */
    
    /* Blank-Out Test Settings */
    trial_start_ms = start_time_ms;
    trial_duration = pattern_trial_duration;
    lookat_A = lookat_A_ms;
    lookaway_A = lookaway_A_ms;
    lookat_B = lookat_B_ms;
    lookaway_B = lookaway_B_ms;
    
		#endif

    // initialization ...
    group_data.load_groups(igroup_data);
		
    max_neuron_id = group_data.get_max_neuron_id();
		
    mfr.resize(max_neuron_id+1,0.0);
		
    //*************************************
    // Set up the motor control system.
    // Assume that each spot in the layer V pyramidal cell array
    // has a specific muscle synergy that is position based.
    // 
    group g = group_data["M1p5(L56)"];
		
    vector<float> one_row;
    one_row.resize(g.cols,0.0f);
		
    shoulder_lut_rad.resize(g.rows,one_row);
    deltoid_lut_rad.resize(g.rows,one_row);
    elbow_lut_rad.resize(g.rows,one_row);
		
    float prow = 0.0f;
    for( int row = 0; row< g.rows; row++)
      {
	float pcol = 0.0f;
	for( int col = 0; col < g.cols; col++ )
	  {
	    shoulder_lut_rad[row][col] = motorprimitives[(int) prow*25+(int)pcol][0] * range_rad[0];
	    deltoid_lut_rad[row][col]  = 1.0 * range_rad[2];//motorprimitives[(int) prow*25+(int)pcol][2] * range_rad[1];//RGM DEBUG 08/08/2012 [2];
	    elbow_lut_rad[row][col]    = motorprimitives[(int) prow*25+(int)pcol][1] * range_rad[2];//RGM DEBUG 08/08/2012 [1];		
	    pcol=pcol+24.999/g.cols;
	  }
	prow=prow+24.999/g.rows;
      }
    fprintf(stderr, "Finished loading world\n");  
  }
	
  ~world() 
    {
      cout << "[WORLD DESTROYER] Quitting now. Goodbye.\n" << flush;
		
      if( myprocid == 0 )
	{
	  //apex.quit();
	  fout.close();
	  patternhist.close();
	}
    };
	
#ifdef APEX3
  void reset()
  {
    apex_client.reset_link();
  }	
#endif
	
  void set_procid(int myid)
  {		
    myprocid = myid;
  }		  
	
	
#ifdef APEX3		
  void init_robot(int myid, ::nsi::thinklink::device* link){
#else
    void init_robot(int myid)
    {		
#endif
			
      myprocid = myid;
			
#ifdef APEX3
    if( myprocid == 0 )
		{
			//initialize robot
			std::string gait_file = "LV_New_Walk_Concat4.bin";
			apex_client.init(gait_file, link);
		}
#else
     apex.init(myid);
#endif
		if( myprocid == 0 )
		{
			fout.open("motorcommands.dat");
			patternhist.open("patternhist.dat");
			DA_debugger.open("DA_debugger.dat");
			button_timings.open("button_timings.dat");
		}
  }
		
    int myrand(int low, int hi)
    {
      return low + rand()%hi;	
    }
		
    float to01(float x,float minx,float maxx)
    {		
      return (x- minx)/(maxx-minx);
    } 
		
    // Returns muscle length as a function of joint angle in radians, given the 
    // length of the tendon insertion points from the joint.
    float length(float a, float b, float theta_rad)
    {
      return sqrt(a*a+b*b-2*a*b*cos(theta_rad));
    }
		
    // Returns derivative of the muscle length with respect to the joint angle (radians),
    // given the length of the tendon insertion points from the joint.
    // This can be used to convert angular joint velocity in radians per second to muscle 
    // length changes in linear units by calculating dtheta*dlength(theta).
    float dlength(float a, float b, float theta_rad)
    {
      return a*b*sin(theta_rad)/sqrt(a*a+b*b-2*a*b*cos(theta_rad));
    }	
		
    //***************************************************************************************************
    void stim_group(string group_name, vector<electrode> electrodes, int t_msec, int trial_duration_msec, float exc_output[])
    {
      group g;
      if( group_data.exists(group_name) )
	{
	  g = group_data[group_name];
	  int gnum=g.last_neuron_id-g.first_neuron_id;
	  int i = g.first_neuron_id;
	  for( int row = 0; row< g.rows; row++)
	    {
	      for( int col = 0; col < g.cols; col++ )
		{
		  for( int elect=0; elect < electrodes.size(); elect ++ )
		    {
		      if( t_msec%trial_duration_msec >= electrodes[elect].start_msec && t_msec%trial_duration_msec <= electrodes[elect].stop_msec)
			{
			  if( dist(x[i], electrodes[elect].position) < electrodes[elect].dist )
			    {
			      exc_output[i] = electrodes[elect].stim_current;
			    }
			}			
		    }							
		  i++;
		}
	    }
	}
			
    }		
				
    //***************************************************************************************************
    // Assumptions: outarray is a a 1d array pixels by pixels long that holds the gabor function output.
    // Sx and Sy is the center of the gabor; theta is the angle, cycles is the number of cycles; all units in radians
    // phase_percent is 0 to 1 and shifts the phase.
    void gabor(float* outarray, int pixels,float Sx,float Sy,float theta, float cycles, float phase_percent, float scale)
    {
      float xphase = phase_percent*cycles*pi*cos(theta);
      float yphase = phase_percent*cycles*pi*sin(theta);
			
      float step = 2*pi/((pixels-1)/cycles);
			
      int c=0;
      for (float x = (-cycles*pi+xphase);x<=(cycles*pi+xphase);x+=step)
      {
        int r=0;
        for (float y = (-cycles*pi+yphase);y<=(cycles*pi+yphase);y+=step)
        {			
          float xPrime = x * cos(theta) + y * sin(theta);
          float yPrime = y * cos(theta) - x * sin(theta);
          outarray[c+r*pixels] = scale*exp(-0.5*((xPrime/Sx)*(xPrime/Sx)+(yPrime/Sy)*(yPrime/Sy)))*cos(xPrime);
          //cout <<" " << outarray[c+r*pixels] << endl;
          r=r+1;
        }
        c=c+1;
      }
    }
		
    //This function allows you to reset neural areas depending on events in the task or environment.  This sample function demonstrates several methods for doing so.  Obviously this function could get every bit as complex as interact()	  
    void reset_interact(int t) 
    {  
      /* KILL THE SIM HERE */ 
      if( kill_sim_at_specified_time )
			{
				if( t >= kill_sim_time_sec*ms_per_sec )
				{
					exit(0);
				}
			}  

      //Phases 1 & 2: J Pattern Training
			if( (t < 170000) && (t%training_sample_duration_ms == 0) )	
			{
				reset_dynamics(); 
			}
															
      //Phase 3: Conditioning
			if( (t >= 170000 && t < 490000) && (t%pattern_trial_duration == 0) )
			{
				reset_dynamics();
			}
						
			//Clear MTS because it might have made a match during the earlier part of the trial
			if( (t >= 170000 && t < 490000) && (t%pattern_trial_duration == 2800) )
			{
				reset_dynamics(group_data["MTSp23M"]);
				reset_dynamics(group_data["MTSb23"]);   
				reset_dynamics(group_data["M1p5(L56)"]);
				reset_dynamics(group_data["M1b5"]);
			}
      
      //Phase 4: Full Control Test
			if( (t >= 490000 && t < 810000) && (t%pattern_trial_duration == 0) )
			{
				reset_dynamics();
			}
						
			//Clear MTS because it might have made a match during the earlier part of the trial
			if( (t >= 490000 && t < 810000) && (t%pattern_trial_duration == 2800) )
			{
				reset_dynamics(group_data["MTSp23M"]);
				reset_dynamics(group_data["MTSb23"]);   
				reset_dynamics(group_data["M1p5(L56)"]);
				reset_dynamics(group_data["M1b5"]);
			}
      
      //PHASE 5: Sequence Training
			if( (t >= 810000 && t <= 904000) && (t%training_sample_duration_ms == 0) )
			{
				reset_dynamics(group_data["V1p23"]);
				reset_dynamics(group_data["V1b23"]); 
			}
      
      //silent period sequence reset
			if(t == 810000 || t == 856000 || t == 902000){
				reset_dynamics();
			}
      
      //Phase 6: Full Match/Non-Match Test
			if( (t >= 905000 && t < 1225000) && (t%pattern_trial_duration == 0) )
			{
				reset_dynamics();
			}
						
			//Clear MTS because it might have made a match during the earlier part of the trial
			if( (t >= 905000 && t < 1225000) && (t%pattern_trial_duration == 2800) )
			{
				reset_dynamics(group_data["MTSp23M"]);
				reset_dynamics(group_data["MTSb23"]);   
				reset_dynamics(group_data["M1p5(L56)"]);
				reset_dynamics(group_data["M1b5"]);
			}
		}
				
    //*******************************	
    // Get the spiked array so we can keep track of spikes. 
    // Get a pointer to the external current array array so we can stimulate neurons.
    // 	set exc_output[neuron] to add exc_output[neuron] to a given neuron number.  (Don't worry if the neuron
    // 	is on this processor or not; we will take care of that in the simulator.)
    // int t in msec.
    // 
    void interact( int t, bool spiked[], float exc_output[], float m1_input[], float s1_input[] )
    {
      static bool first_call = true;
      static float last_shoulder = 0,last_deltoid = 0,last_elbow = 0, last_sdot = 0, last_ddot = 0, last_edot = 0; 
      float shoulder = 0,deltoid = 0,elbow = 0, sdot = 0,ddot = 0,edot = 0, storque = 0, dtorque = 0, etorque = 0;
      bool go_home_flag = true;
      int ms_per_sec = 1000;
      int sec = t/ms_per_sec;
      int gnum,pnum;
      int kk,jj;
      group MTSp23M_group;
			
      int n_patterns = 8;
      //RGM DEBUG 08/08/2012
      //float xo[] = { 0.75,  0.75,  0.75,  0.75,  0.75,  0.75,  0.75,  0.75  };
      //float yo[] = { 0.075, 0.075, 0.075, 0.075, 0.075, 0.075, 0.075, 0.075 };
      
      //float xo[] = { 0.6,  0.6,  0.6,  0.6,  0.6,  0.6,  0.6,  0.6  };
      //float yo[] = {  .2,    .2,     .2,    .2,    .2,     .2,  .2 ,  .2 };
      
      //float xo[] = { 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,  1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8, };
      //float yo[] = {   1,   1,  1,   1,   1, 1.4, 1.4, 1.4, 1.4, 1.4, 1.8, 1.8, 1.8, 1.8, 1.8, 0.2, 0.2, 0.2, 0.2, 0.2, 0.6, 0.6, 0.6, 0.6, 0.6  };
      
      //first one is winner
      float xo[] = { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 };//1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,  1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8 };
      float yo[] = { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 }; // 0.2, 0.2, 0.6, 0.6, 0.6, 0.6, 0.6,   1,   1,  1,   1,   1, 1.4, 1.4, 1.4, 1.4, 1.4, 1.8, 1.8, 1.8, 1.8, 1.8 };
      
      //float xo[] = { 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,  1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8 };
      //float yo[] = { 0.2, 0.2, 0.2, 0.2, 0.2, 0.6, 0.6, 0.6, 0.6, 0.6,   1,   1,  1,   1,   1, 1.4, 1.4, 1.4, 1.4, 1.4, 1.8, 1.8, 1.8, 1.8, 1.8 };
      
      
      //**Match cases, we will move the arm to this location, which was pose 2 in the arm sequence demonstration
      //float xo[] = { 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6 };
      //float yo[] = { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 };
			
      //for V1 testing stimulation
      //float xo[]={.25, .75, 1.25, 1.75, 0, .5, 1, 1.5, 1.8,.2, .6, 1, 1.4, 1.8,.2, .6, 1, 1.4, 1.8,.2, .6, 1, 1.4, 1.8};
      //float yo[]={.2, .2, .2, .2, .6, .6, .6, .6, .6, .6,1,1,1,1,1, 1.4,1.4,1.4,1.4,1.4, 1.8,1.8,1.8,1.8,1.8};
			
      int trial_duration_msec = 0;
      trial_duration_msec = 1*ms_per_sec*n_patterns;
			
      if( sec <= training_end_time_sec )//training phase
			{
				trial_duration_msec = training_sample_duration_ms*n_patterns;
			} 
      else//testing phase
			{
				trial_duration_msec = testing_sample_duration_ms*n_patterns;
				//;3*ms_per_sec*n_patterns; **will probably work only if the learning rates are 'zero' after testing
			}
			
      int pattern_duration_msec = trial_duration_msec/n_patterns;
			
			//calculate DA_decay_rate
			float DA_tau = 500;//ms
			float sd_tau = 1000;//250;//ms
			float tau_window = 50;//ms
			float FV = 0.001;
			float PV = 1.0;
      if( t >= 170000 && t < 490000 )//testing and conditioning
			{
				sd_tau = 1000;//ms
				DA_tau = 2000;//ms
				DA_decay_rate = PV*pow(FV, (1/(DA_tau/tau_window)));
				sd_decay_rate = PV*pow(FV, (1/(sd_tau/tau_window)));
			}		
			else//training, originally 270000
			{
				sd_tau = 6000;
				DA_tau = 1;
				DA_decay_rate = PV*pow(FV, (1/(DA_tau/tau_window)));
				sd_decay_rate = 0.951;
			}
      
      //Expanded pose test: RGM 11/11/10
      #define ORIGINAL_STIM_PATTERN			
			
      //Quick Pick 5 Pattern Test
      float stim_coords[][2] = { xo[12], yo[12],
				   xo[1],  yo[1],
				   xo[3],  yo[3],
				   xo[9],  yo[9] };
      //xo[15], yo[15] };
					
      //1) S1 Stimulation Time Parameters
      int S1_stimulation_start_time_sec = sim_end_time_sec;//0;
      int S1_stimulation_end_time_sec = sim_end_time_sec;   
			
      //1B)
      int S1_stimulation_start_time_sec_B = M1_stim_end_time_sec;// + 50 ;
      int S1_stimulation_end_time_sec_B = M1_stim_end_time_sec;// + 100;    
			
      //2) M1 Stimulation Time Parameters
      int M1_stimulation_start_time_sec = 0;//M1_stim_end_time_sec;//0;// Next, turn off layer 2/3 stimulation and see if reentry from S1 can activate layer 2.
      int M1_stimulation_end_time_sec = M1_stim_end_time_sec;//13000; 
			
      //3) V1 Stimulation Time Parameters
      int V1_stimulation_start_time_sec = V1_stim_start_time_sec;
      int V1_stimulation_end_time_sec = V1_stim_end_time_sec;// change back to sim_end_time_sec before checkin for the real robot
			
      //4) M1 Out Stimulation Time Parameters
      int M1_out_stimulation_start_time_sec = 14000+100;// sim_end_time_sec;
      //int M1_out_stimulation_start_time_sec=1300000;
      int M1_out_stimulation_end_time_sec = sim_end_time_sec-0; // First, turn off motor layer stimulation and see if layer 2/3 still activates layer 5.
			
      // output pattern data
      if( t%pattern_duration_msec == 0 )
      {
				patternhist << t << " " << (t%trial_duration_msec) / pattern_duration_msec << endl << flush;
      }
			
      int steps=11;
      int n_electrodes = steps*2*4;
			
      if( sec >= 0 )
			{				
				#define ROBOT_TESTING_PHASE
				#ifdef ROBOT_TESTING_PHASE
        if( myprocid == 0 )
        {
					if( t%pattern_trial_duration >= 3000 )
					{
						//need to latch until end of current trial
						if( APEX_REWARD == 1 )
						{
							std::cout << "WORLD: REWARD REGISTERED! " << std::endl;;
							reward_button_pressed = true;
						}
					}
					else
					{
						reward_button_pressed = false;
					}
					
					if( t%25 == 0 )
					{
						button_timings << t << " " << reward_button_pressed << endl << flush;
					}
        }
				#endif
				
        
        #ifdef BANANAMANA_RANDOM_SEED_FISHER
        DA_debugger << "************ Random Seed Tester *************" << endl;
        for( int i = 1; i <= 64; ++i )
        {
          DA_debugger << i << ": " << myrand(1,2);
          if(i%2 == 1) DA_debugger << "*";
          DA_debugger << endl;
          if(i%8 == 0) DA_debugger << endl;
        }
        DA_debugger << "************ End of Random Seed Tester *************" << endl;
				exit(0);
        #endif
        
				#define DA_REWARD_ALL_TRIALS
				#ifdef DA_REWARD_ALL_TRIALS
				if( t >= 170000 && t < 490000 )
				{
					/* I expect a MATCH within the 3000 to 4999th milli-second of every 5 second trial */
					if( t%pattern_trial_duration >= 3000 )
					{
						//need to latch until end of current trial
						if( APEX_REWARD == 1 )
						{
							std::cout << "WORLD: REWARD REGISTERED! " << std::endl;;
							reward_button_pressed = true;
						}
					}
					
					/* Update Every 50 milli-seconds */
					if( t%50 == 0 )
					{						
						/* I expect a MATCH within the 3000 to 4999th milli-second of every 5 second trial */
						if( t%pattern_trial_duration >= 3000 )
						{
							/* Flip Coin Once At Beginning of Phase */
							if( t%pattern_trial_duration == 3000 )
							{
								/* myrand will return 1 or 2 */
								int coin = myrand(1,2);
								
								if( coin == 1 )
								{
									coin_flip = true;
								}
								else
								{
									coin_flip = false;//RGM DEBUG
								}
							}
												
							/* Shape Movement w/ M1 stimulation 50%
							 * of the time, only if M1 is on (or MTS
							 * is indirectly on). */
							if(coin_flip)//only coin flip, arm_moved && 
							{
								M1_stim_current = 1800;
							}
							else
							{			
								M1_stim_current = 0;
							}

							/* Reward APE-X only when he moves his 
							 * arm correctly and when thinks he has
							 * a match */
							if( reward_button_pressed && MTS_active ) //RGM DEBUG: realized training durring non-match trials will unlearn match conditioned responses, added MTS_active flag to prevent this
							{
								DA = 0.45;//0.6;//0.85;//1.00;
								cout << "DA ON!" << endl;
							}
							else
							{
								DA = 0.0;
								//cout << "DA OFF!" << endl;
							}
						}
						else
						{//turn off Dopamine for non-match
							M1_stim_current = 0;
							DA = 0.0;
							coin_flip = is_match_trial = reward_button_pressed = false;
						}
						
							DA_debugger << t << " DA: " << DA << " Flip: " << coin_flip << " Current: " << M1_stim_current << " MTS: " << MTS_active 
														<< " Arm Moved: " << arm_moved << " APE-X Button Hit?: " << reward_button_pressed << endl << flush;
					}
				}		
				#endif	
				
				//#define DA_REWARD_MATCH_TRIALS
				#ifdef DA_REWARD_MATCH_TRIALS
				/* Testing And Conditioning Phase */
				if( t >= 270000+320000 && t <= 270000+320000+320000 )
				{
					/* I expect a MATCH within the 3000 to 4999th milli-second of every 5 second trial */
					if( (t-testing_start_time_sec*ms_per_sec)%pattern_trial_duration >= 3000 )
					{
						//need to latch until end of current trial
						if( APEX_REWARD == 1 )
						{
							std::cout << "WORLD: REWARD REGISTERED! " << std::endl;;
							reward_button_pressed = true;
						}
					}
					
					/* Update Every 50 milli-seconds */
					if( t%50 == 0 )
					{
						if( t%pattern_trial_duration == 0 )
						{
							//we are the start of the trial
							++trial_counter;//64 in all
							//we can reinitialize the coin_vec here
						}
						
						/* I expect a MATCH within the 3000 to 4999th milli-second of every 5 second trial */
						if( (t-testing_start_time_sec*ms_per_sec)%pattern_trial_duration >= 3000 )
						{
							/* Flip Coin Once At Beginning of Phase */
							if( (t-testing_start_time_sec*ms_per_sec)%pattern_trial_duration == 3000 )
							{
								/* myrand will return 1 or 2 */
								int coin = myrand(1,2);
								
								cout << ">>>>>>>>>>>>>>>>>>>>>>>>>coin = " << coin << endl;
								
								/* Debugging, to be replaced by some kind of random process */
								/*if( t==273000 || t==313000 || t==353000 || t==393000 || t==453000 ||
										t==493000 || t==533000 || t==573000 )
								{
									coin = 1;
								}
								else
								{
									coin = 0;
								}*/
								
								if( coin == 1 )
								{
									coin_flip = true;
								}
								else
								{
                  coin_flip = true;
								}
								
								/* Determine if this is match-trial */
								if( trial_counter < 32 )
								{//first 32 trials
									if(trial_counter%8 < 4 )
									{//turn on Dopamine for match
										is_match_trial = true;
									}
									else
									{
										is_match_trial = false;
									}
								}
								else
								{//last 32 trials, which are reveresed
									if(trial_counter%8 >= 4 )
									{//turn on Dopamine for match
										is_match_trial = true;
									}
									else
									{
										is_match_trial = false;
									}
								}
							}
												
							/* Shape Movement w/ M1 stimulation 50%
							 * of the time, only if M1 is on (or MTS
							 * is indirectly on). */
							if(coin_flip)//only coin flip, arm_moved && 
							{
								M1_stim_current = 2000;
							}
							else
							{			
								M1_stim_current = 0;
							}

							/* Reward APE-X only when he moves his 
							 * arm correctly and when thinks he has
							 * a match */
							//if( arm_moved_to_target && is_match_trial )
							if( reward_button_pressed && is_match_trial )
							{
								DA = 0.80;//1.00;
								cout << "DA ON!" << endl;
							}
							else
							{
								DA = 0.0;
								//cout << "DA OFF!" << endl;
							}
						}
						else
						{//turn off Dopamine for non-match
							M1_stim_current = 0;
							DA = 0.0;
							coin_flip = is_match_trial = reward_button_pressed = false;
						}
							if( myprocid == 0 )
							{
								DA_debugger << t << " DA: " << DA << " Flip: " << coin_flip << " Current: " << M1_stim_current << " MTS: " << MTS_active 
														<< " Arm Moved: " << arm_moved << " APE-X Button Hit?: " << reward_button_pressed << endl << flush;
							}
					}
				}		
				#endif	

	  //const int motor_update_interval_ms = 250;
	  //const int motor_update_offset_ms = 100;
		#ifdef APEX3
			const int motor_update_interval_ms = 200;//100; //RGM*****
			const int motor_update_offset_ms = 10;//window;
			//#ifdef APEX3
			const int rotation_window = 1000;
			int curr_pose = 0;   
			int num_poses = 4;   	
			
      //DEBUG
      //last deltoid: 1.08146,
			//apex_client.write_read_arm(!first_call, last_shoulder, last_deltoid, last_elbow, shoulder, deltoid, elbow, last_sdot, last_ddot, last_edot, sdot, ddot, edot, storque, dtorque, etorque, t, motor_update_interval_ms, motor_update_offset_ms);
      //apex_client.update_arm_joints(!first_call, last_shoulder, last_deltoid, last_elbow, 500, 1000, 1200, 750, 750, 750, shoulder, deltoid, elbow, sdot, ddot, edot, storque, dtorque, etorque, t, motor_update_interval_ms, motor_update_offset_ms);
      //std::cout << t << ">> shoulder: " << last_shoulder << " deltoid: " << last_deltoid << " elbow: " << last_elbow << std::endl << std::endl; 
  #else			
	  apex.get_arm_proprioception(shoulder,deltoid,elbow, sdot,ddot,edot, storque, dtorque, etorque, t);
	#endif
				
	  sdot=0.0;ddot=0.0;edot=0.0;
				
	  float shoulder_sensory = shoulder;
	  float deltoid_sensory = deltoid; 
	  float elbow_sensory = elbow;			
				
	  //apex.set_arm_command( shoulder, deltoid, elbow, sdot, ddot, edot,t, motor_update_interval_ms, motor_update_offset_ms);
				
	  //apex.get_arm_proprioception(shoulder,deltoid,elbow, sdot,ddot,edot, storque, dtorque, etorque, t);
				
	  //cout << shoulder << " --   " << sdot << endl;
				
	  // convert to appropriate cell types from paper on spinal cord.
	  const float a=1.0,b=.28;  // Muscle tendon insertion points (normalized).
	  float temp, temp2;
	  group g;
				
	  float stim_current = 8000.0;
	  float distance = 0.2;
	  int interval=pattern_duration_msec;
				
	  if( sec >= M1_stimulation_start_time_sec && sec <= M1_stimulation_end_time_sec && group_data.exists("M1p5(L56)") )
	  {
	      g = group_data["M1p5(L56)"];
					
	      float stim_current = M1_stim_current; //RGM DEBUG 08/08/2012 . . .M1_stim_current;
	      float distance = 0.2;
					
	      float x= g.x0;
	      float y= g.y0;
	      float width_mm = g.area_width_mm;
	      vector<electrode> electrodes(n_patterns);
					
				#ifdef ORIGINAL_STIM_PATTERN
				for(kk=0;kk<n_patterns;kk++)
				{					
					electrodes[kk].position[0]=x+xo[kk];//[(n_patterns-1)-kk]; //run patterns backwards to see if maps form early as opposed to forming less for distant arm poses
					electrodes[kk].position[1]=y+yo[kk];//[(n_patterns-1)-kk]; 
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=kk*interval;
					electrodes[kk].stop_msec=(kk+1)*interval-50;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				}
				
				#else
				for(kk=0;kk<n_patterns;kk++) 
				{					
					electrodes[kk].position[0] = x + stim_coords[kk][0];//[(n_patterns-1)-kk]; //run patterns backwards to see if maps form early as opposed to forming less for distant arm poses
					electrodes[kk].position[1] = y + stim_coords[kk][1];//[(n_patterns-1)-kk]; 
					electrodes[kk].position[2] = 0;
					electrodes[kk].start_msec = kk*interval;
					electrodes[kk].stop_msec = (kk+1)*interval-50;
					electrodes[kk].dist = distance;
					electrodes[kk].stim_current = stim_current;
				}
				#endif		
	      stim_group("M1p5(L56)", electrodes, t, trial_duration_msec, exc_output);				
	   }// end M1 Stimulation
				
	  if( sec >= V1_stimulation_start_time_sec && sec <= V1_stimulation_end_time_sec && group_data.exists("V1TCs") )
	  {
	      float distance = 0.2;
	      //float xo[]={.2, .2, 1,   1, 1.8,1.8, .6, .6, 1.4, 1.4,.2, .2, 1, 1, 1.8,1.8, .6, .6,  1.4, 1.4, .2, .2,  1,  1, 1};
	      //float yo[]={.2, .2, .2, .2, .6, .6,  .6, .6, .6, .6,   1, 1,  1,1,  1,   1,   1.4,1.4,1.4, 1.4, 1.8,1.8,1.8,1.8,1.8};
	      //float Xoff[] = { 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,  1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8, 0.2, 0.6,   1, 1.4, 1.8 };
	      //float Yoff[] = { 0.2, 0.2, 0.2, 0.2, 0.2, 0.6, 0.6, 0.6, 0.6, 0.6,   1,   1,  1,   1,   1, 1.4, 1.4, 1.4, 1.4, 1.4, 1.8, 1.8, 1.8, 1.8, 1.8 };
	      float Xoff[] = { 0.93, 0.42, 0.95, 1.40 };
	      float Yoff[] = { 1.16, 0.71, 0.45, 0.9 };
					
	      //float distance[]={0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4};
	      float stim_current = 400;//400
	      float start_offset=50;//50
	      float end_offset=50;
	      g = group_data["V1TCs"];
					
	      float x= g.x0;
	      float y= g.y0;
	      float width_mm = g.area_width_mm;
	      vector<electrode> electrodes(n_patterns);
					
					
	      for(kk=0;kk<n_patterns;kk++)
	      {
					electrodes[kk].position[0]=x+Xoff[kk];//x+xo[kk]; 
					electrodes[kk].position[1]=y+Yoff[kk];//y+yo[kk]; 
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=kk*interval; //added to offset visual stimulus to align with S1 stimulus
					electrodes[kk].stop_msec=(kk+1)*interval-10;//50
					//electrodes[kk].dist=distance[kk];
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
	      }							
					
	      /* Sequence V1 Stimulation Paradigm */
	      int pattern = (t/interval)%n_patterns;
					
	      if( sec < testing_start_time_sec )
				{
					//present artificial visual stimulus, could present real stimulus during this phase
					stim_group("V1TCs", electrodes, t, trial_duration_msec, exc_output);
				}
	      else if( sec >= M1_stim_end_time_sec && sec < first_seq_stim_start_sec )
				{
					//present artificial visual stimulus
					stim_group("V1TCs", electrodes, t, trial_duration_msec, exc_output);
				}
	      else if( sec >= first_seq_stim_start_sec )
				{
					// Stimulate only the first pattern in the sequence and test for recall.
					if( pattern == 0)
					{
						//stim_group("V1TCs", electrodes, t, trial_duration_msec, exc_output);
						//stim_group("S1p23", electrode_s1, t, pattern_duration_msec, exc_output);
					}
				// else don't stimulate anything.
				}
	      else
				{
					//do nothign for now . . .
					//stim_group("V1TCs", electrodes, t, trial_duration_msec, exc_output);
					//stim_group("S1p23", electrode_s1, t, pattern_duration_msec, exc_output);
				}		
	    }// V1 Stimulation
				
	  //********************************************************
	  if( ( ((sec >= S1_stimulation_start_time_sec) && (sec <= S1_stimulation_end_time_sec)) || 
				((sec >= S1_stimulation_start_time_sec_B) && (sec <= S1_stimulation_end_time_sec_B)) ) && 
	      (group_data.exists("S1p23")) )
		{
					
			float stim_current=800;
	    const bool use_gabor = false;
	    const bool use_linear = true;
	    if( use_gabor )
			{
		  // Use gabor functions to test the mapping capabilities of the network.
		  g = group_data["S1TCs"];
						
		  float cycles = 1.0;
		  float phase_percent = 0.0f;
						
		  int pattern = (t/interval)%n_patterns;
		  gabor( exc_output+g.first_neuron_id, g.rows, (float) g.rows/2, (float) g.cols/2, (float)pattern/(float)n_patterns*pi, cycles, phase_percent, stim_current);
		  // rectify
		  for(int i = g.first_neuron_id;i<g.last_neuron_id;i++)
		    {
		      exc_output[i] = max(exc_output[i],0.0f);
		    }
		}
	      else if (use_linear) // use linear representation of angles on two dimentional S1TCs array -- just like in V1TCs 12-01-10
		{  
		  g = group_data["S1TCs"];
		  gnum=g.last_neuron_id-g.first_neuron_id;
		  float x= g.x0;
		  float y= g.y0;
		  float width_mm = g.area_width_mm;
		  stim_current=1000;
		  distance = 0.2;
						
		  vector<electrode> electrode_s1(1);
		  electrode_s1[0].position[0] = x+2*deltoid_sensory*width_mm/pi/1.1+0.2; 
		  electrode_s1[0].position[1] = y+2*shoulder_sensory*width_mm/pi/1.1+0.2; 
		  electrode_s1[0].position[2] = 0;
		  electrode_s1[0].dist = distance;
		  electrode_s1[0].stim_current = stim_current;
						
		  if( sec <= M1_stim_end_time_sec )//M1_stim_training_sec )//training phase
		    {
		      electrode_s1[0].start_msec = 50;//50
		      electrode_s1[0].stop_msec = pattern_duration_msec-10;// had to turn off proprioception sensory input to S1TCs -50ms to 50 around pattern swithch time - 12-1/10 -Ychen  
		    }
		  else//testing phase
		    {
		      electrode_s1[0].start_msec = 10; //100
		      electrode_s1[0].stop_msec = pattern_duration_msec-10;//-50;  // had to turn off proprioception sensory input to S1TCs -50ms to 50 around pattern swithch time - 12-1/10 -Ychen  
		    }
						
		  stim_group("S1TCs", electrode_s1, t, pattern_duration_msec, exc_output);
			}			
	   }// end S1 Stimulation Section
				
	  //******************  Motor babbling.  Stimulate the Layer V cells to get it going.	
	  if( sec >= M1_out_stimulation_start_time_sec && sec <= M1_out_stimulation_end_time_sec && group_data.exists("M1p5(L56)") )
	  {
			g = group_data["M1p5(L56)"];
				
	    float stim_current = 2000.0;
	      float distance = 0.2;
					
	      float x= g.x0;
	      float y= g.y0;
	      float width_mm = g.area_width_mm;
	      vector<electrode> electrodes(n_patterns);
					
	      for(kk=0;kk<n_patterns;kk++)	
	      {
					electrodes[kk].position[0]=x+xo[kk]; 
					electrodes[kk].position[1]=y+yo[kk]; 
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=kk*interval;
					electrodes[kk].stop_msec=(kk+1)*interval-50;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				}
					
	      stim_group("M1p5(L56)", electrodes, t, trial_duration_msec, exc_output);
	      //stim_group("M1p5(L23)", electrodes, t, trial_duration_msec, exc_output);
	      //stim_group("M1b5", electrodes, t, exc_output);  //Petersen & Sakman ,2001 discussion says that fewer inhib cells likely activated directly by microstim.
	      //stim_group("M1nb5", electrodes, t, exc_output);				
	    }		
				
	  /* Part II.  Set the motor commands based on Layer V cell activity. */
				
	  shoulder = 0.0f;
	  deltoid = 0.0f; 
	  elbow = 0.0f;			
				
	  //***RGM: Place to look at for robot Mental Rotation Test
	  g = group_data["M1p5(L56)"];
	  gnum=g.last_neuron_id-g.first_neuron_id;
				
	  int window = 10;
	  float tau = exp(-1.0/(float)window);  // 100 msec time constant; trying to match time constant of slow twitch muscles.
				
	  //if( myprocid == 0 )
	  //{
	      shoulder = 0.0f;
	      deltoid = 0.0f; 
	      elbow = 0.0f;
	      float rate_sum = 0.0f;
	      // 1. Interpret activity
	      int i = g.first_neuron_id;
	      int votes = 0;
					
	      for(int row = 0; row< g.rows; row++)
				{
					for( int col = 0; col < g.cols; col++ )
					{
						mfr[i]= tau*mfr[i] + (1.0f-tau)*(spiked[i]?1.0:0.0)*1000 ; //This gives mean rate in Hz.
							
		      if( mfr[i] > 10.0 )
					{  // only vote if you're serios about it.
								
						// Generate population vector which will determine instantaneous posture target.						
						shoulder += mfr[i] * shoulder_lut_rad[row][col];
						deltoid  += mfr[i] * deltoid_lut_rad[row][col];
						elbow    += mfr[i] * elbow_lut_rad[row][col];
						rate_sum += mfr[i];
						votes++;
					}					
		      //if(t%pattern_duration_msec == window && myprocid==0) cout <<fixed << setw(5) << setprecision(1)  << mfr[i] << " " ;					
		      i++;
		    }
		  //if(t%pattern_duration_msec == window && myprocid==0) cout << endl ;
			}
	      //if(t%pattern_duration_msec == window && myprocid==0) cout << endl ;			
					
			if( rate_sum > 0.000001 && votes >= 10 )
			{
				shoulder *= 1.0f/rate_sum;
				deltoid *= 1.0f/rate_sum;
				elbow *= 1.0f/rate_sum;		
							
				// MTS matched -> move arm
				//shoulder = 0.124802;
				//deltoid = 0.176147;
				//elbow = 0.32725;
							
				go_home_flag = true;
				match_flag = true;
			}
			else // Go To Home Position
			{
				shoulder = home_pose_radians[0];
				deltoid = home_pose_radians[1];
				elbow = home_pose_radians[2];	

				//shoulder = 0.851359;
				//deltoid = 1.08146;
				//elbow = 0.32725;
				match_flag = false;
			}
	      sdot= fabs(shoulder-shoulder_sensory)*max_velocity_rad_s* 0.5;
	      ddot= fabs(deltoid-deltoid_sensory)*max_velocity_rad_s* 0.89;
	      edot= fabs(elbow-elbow_sensory)*max_velocity_rad_s* 0.5;  // 10% of max velocity.
					
	      //Write Joint Commands and Sensory Data To motorcommands.dat
				if( (t%50) == 0 )
				{
					fout << t << ',' << shoulder << ',' << deltoid << ',' << elbow << ',';
					fout << shoulder_sensory << ',' << deltoid_sensory<< ',' << elbow_sensory << endl << flush;
				}
			
				MTSp23M_group = group_data["MTSp23M"];
				
				window = 10;
				tau = exp(-1.0/(float)window);  // 100 msec time constant; trying to match time constant of slow twitch muscles.
			
	      float MTS_rate_sum = 0.0f;
	      int MTS_votes = 0;
					
	      for( int i = MTSp23M_group.first_neuron_id; i < MTSp23M_group.last_neuron_id; i++ )
				{
						mfr[i]= tau*mfr[i] + (1.0f-tau)*(spiked[i]?1.0:0.0)*1000 ; //This gives mean rate in Hz.
							
						if( mfr[i] > 10.0 )
						{ 
							MTS_rate_sum += mfr[i];
							MTS_votes++;
						}					
		    }	
					
				//divide out mfr by number of neurons to get*** average mfr for DA
				if( MTS_rate_sum > 0.000001 && MTS_votes >= 10 )
				{
					MTS_active = true;
				}
				else // Go To Home Position
				{
					MTS_active = false;
				}
					
				/* Arm Moved at all? */
				start_pose = home_pose_radians[0] + home_pose_radians[1];
				current_pose = shoulder + deltoid;
				if( abs(current_pose-start_pose) > movement_threshold )
				{
					 arm_moved = true;
				}
				else
				{
					arm_moved = false;
				}
						
				/* Arm Moved To Target Location? */
				start_pose = target_pose_radians[0] + target_pose_radians[1];
				current_pose = shoulder + deltoid;
				if( abs(current_pose-start_pose) <= target_threshold )
				{
					arm_moved_to_target = true;
				}
				else
				{
					arm_moved_to_target = false;
				}
		
					
	      //**** 2) Move the arm by writing specified commands. ****
			if( sec >= 0 )
			{
				last_shoulder = shoulder;
				last_deltoid = deltoid;
				last_elbow = elbow;
				last_sdot = sdot;
				last_ddot = ddot;
				last_edot = edot;
							
				const int motor_update_interval_ms = 250;
				const int motor_update_offset_ms = window;			
						
#ifdef APEX3
		  /* Robot and Rotation Station Control */
		  if( myprocid == 0 )
		  {			 
        //Robot Rotation Training Phase 
        if((t >= 810000 && t < 904000) && (t%training_sample_duration_ms == 900))
        {
          int record_pattern = 0; 
                    
          if( t == 855900 )
          {
            record_counter = 0;
          }
          else
          {
            ++record_counter;
          }
          
          record_pattern = record_counter%4;
          //J Pattern Sequence Training
          if( t >= 810000 && t <= 856000 )
          {
            apex_client.set_position(SPINNER_ID, spinner_object1, 300, 700); //set rotation
          }
          else
          {
            apex_client.set_position(SPINNER_ID, spinner_object2, 300, 700); //set rotation
          }
          apex_client.set_position(ROTATION_ID,rot_sta_poses[record_pattern], 300, 700); //set rotation 
          
          //apex_client.write_read_arm(!first_call, home_pose_radians[0], home_pose_radians[1], home_pose_radians[2], shoulder, deltoid, elbow, last_sdot, last_ddot, last_edot, sdot, ddot, edot, storque, dtorque, etorque, t, 50, 0);//interval, offset
          apex_client.update_arm_joints(!first_call, home_pose_radians[0], home_pose_radians[1], home_pose_radians[2], 500, 1000, 1200, 750, 750, 750, shoulder, deltoid, elbow, sdot, ddot, edot, storque, dtorque, etorque, t, 50, 0);
                    
          //give time for new pattern to rotate into place to prevent blurring
          //usleep(3000000);
        }
        
        //Full Testing & Conditioning Phases
        if( (t >= 170000 && t < 490000) || (t >= 490000 && t < 810000) || (t >= 905000 && t < 1225000 ) )
        {
          //Return Head to home position and Present Pattern A
          if( t%(pattern_trial_duration) == lookat_A_ms )
          {  					
            head_pose = head_pose_home;
 
            if(t == 170000 || t == 490000 || t == 905000)//905000
            {
              pattern_A = 0;
              pattern_B = 0;
              apex_client.set_position(ROTATION_ID,rot_sta_poses[pattern_A], 300, 700); 
              apex_client.set_position(SPINNER_ID, spinner_object1, 300, 700); 
              cerr << endl << "[1] Set Head(12) to home_position for Pattern_A: " << t << endl << flush;		
              motor_flag = true;
              trial_counter = -1;
              conditioning_cnt = 0;
            }
            
            match_flag = false;
            first_match = false;
            /*Beginning of trial */
            ++trial_counter;
            std::cout << "Trial count: " << trial_counter << std::endl;
          }
          //Turn Head for Delay and Mental Rotation
          else if( (t%pattern_trial_duration == (lookaway_A_ms + 100)) ) 
          {           
            //set blinder down
            apex_client.set_position(BLINDER_ID, blinder_down, 300, 700);
                        
            cerr << endl << "[2] Set Head(12) to look_away during Delay_A: " << head_pose << " @ " << t << endl << flush;
            motor_flag = true;
          }
          //Present Pattern B while head is turned
          else if(t%pattern_trial_duration == (lookaway_A_ms + 300))
          {
            /* Set Rotation */
            apex_client.set_position(ROTATION_ID, rot_sta_poses[pattern_B], 300, 700);
            
            //Set-up Spinner for Pattern-B
            if(pattern_B > 3)
            {
              apex_client.set_position(SPINNER_ID, spinner_object2, 306, 700);
            }
            else
            {
              apex_client.set_position(SPINNER_ID, spinner_object1, 306, 700);
            }
            
            if(t >= 170000 && t < 490000)//conditioning phase
            {
              cout << "******** Conditioning Count: " << conditioning_cnt << " Pattern A: " << pattern_A <<  " Pattern B: " << pattern_B << endl << endl;
              ++conditioning_cnt;
              if(conditioning_cnt%2 == 0)
              {
                ++pattern_A;
                if(pattern_A > max_pattern+4)
                {
                  pattern_A = min_pattern;
                }
                pattern_B = pattern_A;
              }
              else
              {
                //we're odd
                if(pattern_A > 3)
                {
                  pattern_B = pattern_A - 4;
                }
                else
                {
                  pattern_B = pattern_A + 4;
                }
                /*if(conditioning_cnt%2 == 0) //even
                {
                  pattern_B = pattern_A;
                }
                else//we're odd
                {
                  if(pattern_A > 3)
                  {
                    pattern_B = pattern_A - 4;
                  }
                  else
                  {
                    pattern_B = pattern_A + 4;
                  }
                }*/
              }
            }
            else
            {
              ++pattern_B;
              if(pattern_B > max_pattern+4)
              {
                pattern_B = min_pattern;
                ++pattern_A;     
                if(pattern_A > max_pattern+4)
                {
                  pattern_A = min_pattern;
                }
              }
            }
            
            /* Set Spinner 
            if( trial_counter >= 0 && trial_counter < 16 )
            {
              apex_client.set_position(SPINNER_ID, spinner_object1, 306, 700);
            }
            else if( trial_counter >= 16 && trial_counter < 32 )
            {
              apex_client.set_position(SPINNER_ID, spinner_object2, 306, 700);
            }
            else if( trial_counter >= 32 && trial_counter < 47 )
            {
              apex_client.set_position(SPINNER_ID, spinner_object2, 306, 700);
            }
            else if( trial_counter >= 48 && trial_counter < 64 )
            {
              apex_client.set_position(SPINNER_ID, spinner_object1, 306, 700);
            }
            */

            cerr << endl << "[3] Set Station(ROTATION_ID) to Pattern_B: " << t << endl << flush;
            motor_flag = true;
          }
          //Turn Head to Home Position to see Pattern B
          else if( t%pattern_trial_duration == (lookat_B_ms - 100) )
          {
            //head_pose = head_pose_home;
            //apex_client.set_position(12, head_pose, 300, 700);
            
            //set blinder up
            apex_client.set_position(BLINDER_ID, blinder_up, 300, 700);
            
            cerr << endl << "[4] Set Head(12) to home_position for Pattern_B: " << head_pose << " @ " << t << endl << flush;
            motor_flag = true;
          }
          //Set Blinder Down for Response Period
          else if( t%pattern_trial_duration == (lookat_B_ms + 1000) )
          {					
            //set blinder down
            apex_client.set_position(BLINDER_ID, blinder_down, 300, 700);
            
            motor_flag = true;
          }
          else if( t%pattern_trial_duration == (pattern_trial_duration - 100) )
          { 
            /* Set Rotation Station */
            apex_client.set_position(ROTATION_ID,rot_sta_poses[pattern_A], 300, 700);
            
            //Set-up Spinner for Pattern-B
            if(pattern_A > 3)
            {
              apex_client.set_position(SPINNER_ID, spinner_object2, 306, 700);
            }
            else
            {
              apex_client.set_position(SPINNER_ID, spinner_object1, 306, 700);
            }
            
            /* Set Spinner 
            if( trial_counter >= 0 && trial_counter < 15 )
            {
              apex_client.set_position(SPINNER_ID, spinner_object1, 306, 700);
            }
            else if( trial_counter >= 15 && trial_counter < 31 )
            {
              apex_client.set_position(SPINNER_ID, spinner_object2, 306, 700);
            }
            else if( trial_counter >= 31 && trial_counter < 47 )
            {
              apex_client.set_position(SPINNER_ID, spinner_object1, 306, 700);
            }
            else if( trial_counter >= 47 && trial_counter < 63 )
            {
              apex_client.set_position(SPINNER_ID, spinner_object2, 306, 700);
            }
            */
            //set blinder up
            apex_client.set_position(BLINDER_ID, blinder_up, 300, 700);
            
            cerr << endl << "[6] Set Station(ROTATION_ID) to Pattern_A for next trial: " << t << endl << flush;
            motor_flag = true;
          }
        }		
      }//end if myprocid   
						
		  /* Command Head and/or Rotation Station */
    //Full Testing & Conditioning Phases
      if( (t >= 170000 && t < 490000) || (t >= 490000 && t < 810000) || (t >= 905000 && t < 1225000 ) )
      {
        if(motor_flag)
        {
          cerr << endl << "Call read/write arm method BEFORE matching " << t << endl << flush;
          apex_client.update_arm_joints(!first_call, home_pose_radians[0], home_pose_radians[1], home_pose_radians[2], 500, 1000, 1200, 750, 750, 750, shoulder, deltoid, elbow, sdot, ddot, edot, storque, dtorque, etorque, t, t, 0);
          motor_flag = false;
        }
              
        if( t%pattern_trial_duration >= lookaway_B_ms && match_flag)
        {       
          if( t%ms_per_sec == 0 )
          {
            //cout << "2) Updating Motor Behavior: t = " << t << " Pattern A = " << pattern_A << " Pattern B = " << pattern_B << endl << endl;
          } 
          if( !first_match )
          {
            cerr << endl << "[5***] Call read/write arm method DURING matching " << t << endl << flush;
            apex_client.update_arm_joints(!first_call, last_shoulder, last_deltoid, last_elbow, 500, 1000, 1200, 750, 750, 750, shoulder, deltoid, elbow, sdot, ddot, edot, storque, dtorque, etorque, t, t, 0);
            first_match = true;
          }
            match_flag = false;
        }
      }
						
#else
		  //Simulated Robot
      //RGM DEBUG 08/08/2012
		  //apex.set_arm_command(shoulder, deltoid, elbow, sdot, ddot, edot,t, motor_update_interval_ms, motor_update_offset_ms);
		  //cout << "Poses *********** Shoulder= " << shoulder << " Deltoid= " << deltoid << endl << endl << flush;
#endif
		}
	    //}//myprocid == 0
	}//if (sec>=0)
      first_call = false;
    }// interact()
		
    int get_sim_end_time_sec()
    {
      return sim_end_time_sec;
    }
		
    int sim_end_time_sec;
    ofstream fout, patternhist;
#ifdef APEX3
		
    //apex_robot< typename RobotType >  apex_client;
    apex_robot< ::nsi::thinklink::remote_robot<> >  apex_client;
    int trial_start_ms;
    int trial_duration;
    int lookat_A;
    int lookaway_A;
    int lookat_B;
    int lookaway_B;
#endif
		
#ifdef APEX2
    robot apex;
#endif
		
    int max_neuron_id;
    groups group_data;
    int count_fired;
    int myprocid;
		
    vector<vector<float> > shoulder_lut_rad;
    vector<vector<float> > deltoid_lut_rad;
    vector<vector<float> > elbow_lut_rad;
  };// class world 
